// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GraphOptimizationTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Numerics.GraphOptimization;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Initializes a new instance of the PoseTest class.
    /// </summary>
    [TestClass]
    public class GraphOptimizationTest
    {
        /// <summary>
        /// Test the graph optimization constructor
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphConstructor()
        {
            Graph<Pose2D, Pose2DEdge> graph = new Graph<Pose2D, Pose2DEdge>();
            Assert.IsTrue(graph.Nodes != null && graph.Edges != null, "Graph Constructor Failed");
        }

        /// <summary>
        /// Test Adding edges and nodes to a graph
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphAddNodesAndEdges()
        {
            Graph<Pose2D, Pose2DEdge> graph = new Graph<Pose2D, Pose2DEdge>();
            graph.AddNode(new Pose2D(0, 0, 0));
            graph.AddNode(new Pose2D(2, 0, 0));
            graph.AddNode(new Pose2D(2, 2, Math.PI / 2));
            graph.AddNode(new Pose2D(0, 2, Math.PI));

            graph.AddEdge(new Pose2DEdge(new Pose2D(2, 0, 0), 0, 1));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 1, 2));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 2, 3));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI), 0, 3));

            Assert.IsTrue(graph.Nodes.Count == 4 && graph.Edges.Count == 4, "Adding nodes and edges failed");
        }

        /// <summary>
        /// Test Optimization for ground truth
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphOptimizeGroundTruth()
        {
            Graph<Pose2D, Pose2DEdge> graph = new Graph<Pose2D, Pose2DEdge>();
            graph.AddNode(new Pose2D(0, 0, 0));
            graph.AddNode(new Pose2D(2, 0, 0));
            graph.AddNode(new Pose2D(2, 2, Math.PI / 2));
            graph.AddNode(new Pose2D(0, 2, Math.PI));

            graph.AddEdge(new Pose2DEdge(new Pose2D(2, 0, 0), 0, 1));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 1, 2));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 2, 3));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI), 0, 3));
    
            graph.Optimize(Pose2DEdge.GlobalPoseError, Pose2DEdge.GlobalPoseIncrement);

            double[] total_error = new double[3];
            for (int i = 0; i < 3; i++)
            {
                total_error[i] = 0;
            }

            foreach (Pose2DEdge edge in graph.Edges)
            {
                double[] error = new double[3];
                Pose2DEdge.GlobalPoseError(graph.Nodes[edge.IndexI], graph.Nodes[edge.IndexJ], edge, ref error);
                for (int i = 0; i < 3; i++)
                {
                    total_error[i] += error[i];
                }
            }

            Vector3 error_norm = new Vector3(total_error[0], total_error[1], total_error[2]);

            Assert.IsTrue(error_norm.Length() < 0.000001, "Optimization of Ground Truth Failed");
        }

        /// <summary>
        /// Test Optimization for noisy data
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GraphOptimizeNoisyData()
        {
            Graph<Pose2D, Pose2DEdge> graph = new Graph<Pose2D, Pose2DEdge>();
            graph.AddNode(new Pose2D(0, 0, 0));
            graph.AddNode(new Pose2D(2, -2, 0));
            graph.AddNode(new Pose2D(8, 5, Math.PI));
            graph.AddNode(new Pose2D(1, -3, 2 * Math.PI));

            graph.AddEdge(new Pose2DEdge(new Pose2D(2, 0, 0), 0, 1));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 1, 2));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI / 2), 2, 3));
            graph.AddEdge(new Pose2DEdge(new Pose2D(0, 2, Math.PI), 0, 3));

            graph.Optimize(Pose2DEdge.GlobalPoseError, Pose2DEdge.GlobalPoseIncrement);

            double[] total_error = new double[3];
            for (int i = 0; i < 3; i++)
            {
                total_error[i] = 0;
            }

            foreach (Pose2DEdge edge in graph.Edges)
            {
                double[] error = new double[3];
                Pose2DEdge.GlobalPoseError(graph.Nodes[edge.IndexI], graph.Nodes[edge.IndexJ], edge, ref error);
                for (int i = 0; i < 3; i++)
                {
                    total_error[i] += error[i];
                }
            }

            // no convergence testing in graph optimizer yet, so this isn't guranteed to reach final solution in 5 iterations
            // on the todo list
            Vector3 error_norm = new Vector3(total_error[0], total_error[1], total_error[2]);

            Assert.IsTrue(error_norm.Length() < 0.1, "Optimization of Noisy Data Failed");
        }
    }
}
